package twoD.IsoPfem.Control;

public class DisplacementConstraintCondition implements IConstraintCondition {

	private double s_;
	private double delta_s, delta_s_;
	private int[] dofs_control;
	private double lambda0;

	public DisplacementConstraintCondition(double norm_u_hat_0,
			double delta_u_hat, int... dofs_control) {
		delta_s = delta_u_hat;
		s_ = norm_u_hat_0;
		this.dofs_control = dofs_control;
	}

	@Override
	public double f(double[] u, double[] uold, double lambda, double lambda_old) {
		double tmp = 0;
		for (int i : dofs_control)
			tmp += u[i] * u[i];
		tmp = Math.sqrt(tmp);
		return tmp - s_;
	}

	@Override
	public double[] dfdu(double[] u, double[] uold, double lambda,
			double lambda_old) {
		double[] tmp = new double[u.length];
		double norm_u = f(u, uold, lambda, lambda_old) + s_;
		for (int i : dofs_control)
			tmp[i] = u[i] / norm_u;
		return tmp;
	}

	@Override
	public double dfdlambda(double[] u, double[] uold, double lambda,
			double lambda_old) {
		return 0;
	}

	@Override
	public void increment() {
		s_ += delta_s;
	}

	@Override
	public void setInitialLambda(double lambda0) {
		this.lambda0 = lambda0;
	}

	@Override
	public double getInitialLambda() {
		return lambda0;
	}

	@Override
	public double getS() {
		return s_;
	}

	@Override
	public double[] getU(double[] uGlobal) {
		double[] u = new double[dofs_control.length];
		for (int i = 0; i < dofs_control.length; i++)
			u[i] = uGlobal[dofs_control[i]];
		return u;
	}

	@Override
	public void storeStepSize() {
		delta_s_ = delta_s;
	}

	@Override
	public void restoreStepSize() {
		delta_s = delta_s_;
	}

	@Override
	public void reduceStepSize() {
		delta_s /= 2;
		// TODO
	}
}
